#pragma once
#include <iostream>
#include <Eigen/Dense>
#include <memory>
#include "vehicle_dyn_base.hpp"
#include "OsqpSolver/OsqpSolver.h"

constexpr double g = 9.86;
namespace auto_ros
{
namespace control
{
class VehicleLinear4d4s : public VehicleDynBase
{
public:
	//init state fun when speed is 1
	VehicleLinear4d4s(std::string dyn_yaml_path_name);
	void para_update_matrix(double speed);
	double mass();
	double lf();
	double lr();
	double cf();
	double cr();
	double iz();
	double h();
	double lw();
	double max_tire_f_angle();
	double speed_threshold();
	bool set_mass(double mass);
	bool set_lf(double lf);
	bool set_lr(double lr);
	bool set_cf(double cf);
	bool set_cr(double cr);
	bool set_iz(double iz);
	bool set_h(double h);
	bool set_lw(double lw);
	bool set_max_lateral_acc(double max_acc);
	bool set_max_tire_f_angle(double max_angle);
	virtual bool calc_steady_state_ctr(
		const Eigen::VectorXd &distur_ref, const Eigen::VectorXd &y_controlled_ref,
		Eigen::VectorXd &steady_state, Eigen::VectorXd &steady_control,
		std::map<std::string, double> other_par_map = {});

private:
	//paramter
	double mass_;
	double cf_;
	double cr_;
	double lf_;
	double lr_;
	double iz_;
	double h_;
	double lw_;
	double road_mu_;
	//local parameter
	double c_yaw_rate_;
	double c_beta_;
	double c_delta_f_;
	double c_delta_r_;
	double d_yaw_rate_;
	double d_beta_;
	double d_delta_f_;
	double d_delta_r_;
	double d_l_;
	//steady state and control variable
	Eigen::VectorXd steady_state_;
	Eigen::VectorXd steady_control_;
	double max_lateral_acc_;
	double max_tire_f_angle_;
	//state
	double speed_;
	double speed_threshold_; // divided by 0 error
	//osqp related
	OsqpSolver::Solver ref_qp_solver_;
	Eigen::MatrixXd hessian_;
	Eigen::VectorXd gradient_;
	Eigen::MatrixXd constraint_matrix_;

	Eigen::VectorXd lowerBound_;
	Eigen::VectorXd upperBound_;
	bool init_ref_solver();
};
} // namespace control
} // namespace auto_ros